import sys
sys.path.append("")
from geometry import Vector,Quaternion
import math

class Coord_fix():
    def __init__(self,quat,offset):
        self.scale = 100
        self.init_q = quat 
        if len(offset) < 3:
            offset = [0,0,0]
        self.offset = Vector(offset[0],offset[1],offset[2])

    def pos_fix(self,quat,pos):

        # 修正当前姿态四元数，保证初始姿态为[1,0,0,0]
        cur_q = Quaternion()
        cur_q.set_value(quat)
        cur_q = Quaternion.mul(cur_q.get_adjoint(),self.init_q)
        cur_q.switch_xz()
        cur_q.multiply_list([1,-1,1,1])

        #cur_q.printq()

        # 计算当前姿态下的偏移向量
        cur_vec = Quaternion.rotate(cur_q,self.offset)

        # 修正当前传感器坐标，由传感器坐标系变换到机器人坐标系
        cur_pos = Vector(pos.x*self.scale, pos.y*self.scale, pos.z*self.scale)

        #cur_vec.printv()
        #cur_pos.printv()

        # 修正当前位置坐标
        fixed_pos = Vector.minus(cur_pos,cur_vec)
        fixed_pos = Vector.add(fixed_pos,self.offset)
        return fixed_pos

class Projection():
    def __init__(self,quat):
        self.init_q = Quaternion(*quat) 
        self.offset = Vector(0,0,1)

        self.h = 0.0
        self.sin_a = 0.0
        self.cos_ori = 0.0
        self.sin_ori = 0.0
        self.x_scale = 0.0
        self.y_scale = 0.0
        self.x_bias = 0.0
        self.y_bias = 0.0


    def pre_transform(self,quat,h = 120):
        cur_q = Quaternion(*quat)
        # cur_q.set_value(quat)
        cur_q = Quaternion.mul(cur_q.get_adjoint(),self.init_q)
        cur_q.switch_xz()
        cur_q.multiply_list([1,-1,1,1])

        # cur_q.printq()

        # 计算当前姿态下的偏移向量
        cur_vec = Quaternion.rotate(cur_q,self.offset)
        # cur_vec.printv()

        # a: 相较于水平状态的倾角
        cos_a = cur_vec.cosv(self.offset)
        sin_a = math.sqrt(1 - cos_a * cos_a)

        t_len_norm = math.sqrt(cur_vec.x * cur_vec.x + cur_vec.y * cur_vec.y)
        t_len = - t_len_norm * h / cur_vec.z

        if t_len_norm < 0.01:
            ori = 0.0
        else:
            ori = math.atan2(cur_vec.y, cur_vec.x)
        cos_ori = math.cos(ori)
        sin_ori = math.sin(ori)
        # h = 1.0

        # print('cos_a:{},ori:{},len:{}'.format(cos_a, ori/math.pi*180, t_len))

        self.x_scale = h / math.pow(cos_a,2) + t_len * sin_a
        self.y_scale = h / cos_a

        self.x_bias = h * t_len * cos_ori
        self.y_bias = h * t_len * sin_ori
        self.sin_ori = sin_ori
        self.cos_ori = cos_ori
        self.sin_a = sin_a
        self.h = h

    @staticmethod
    def rangeLimit(value, range):
        if value > range:
            value = range
        elif value < -range:
            value = -range
        return value

    def transform(self, pos, limit = 10000):
        ox = pos[0]
        oy = pos[1]
        cos_ori = self.cos_ori
        sin_ori = self.sin_ori

        h = self.h
        x = (ox * cos_ori + oy * sin_ori)
        y = (- ox * sin_ori + oy * cos_ori)
        base = x * self.sin_a + h
        

        x_res = - (x * self.x_scale * cos_ori - y * self.y_scale * sin_ori + self.x_bias) / base
        y_res = - (x * self.x_scale * sin_ori + y * self.y_scale * cos_ori + self.y_bias) / base

        x_res = Projection.rangeLimit(x_res, limit)
        y_res = Projection.rangeLimit(y_res, limit)

        return [-x_res, -y_res]

